%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.

planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end

%%



load('SE_L4_L5_Lyapunov.mat');

load('SE_L4_Vertical.mat');



for ii = 1:length(T_L4_VL)
    Jacobi_Save_VL(ii) = jacobiConst(IC_L4_VL(1:3,ii),IC_L4_VL(4:6,ii),u);

    Xo_VL = IC_L4_VL(:,ii);

    S_sci_VL = C2I_primary(Xo_VL,u,DU,VU,0);
    COE_VL(:,ii) = State2Coe(S_sci_VL,GM_central);
end

for ii = 1:length(T)
    Jacobi_Save_PL(ii) = jacobiConst(IC_L4(1:3,ii),IC_L4(4:6,ii),u);

    Xo_PL = IC_L4(:,ii);

    S_sci_PL = C2I_primary(Xo_PL,u,DU,VU,0);
    COE_PL(:,ii) = State2Coe(S_sci_PL,GM_central);
end
load('SEL1_Lyapunov.mat')
for ii = 1:length(T)
    Jacobi_Save_L1(ii) = jacobiConst(IC(1:3,ii),IC(4:6,ii),u);

    Xo_PL = IC(:,ii);

    S_sci_L1 = C2I_primary(Xo_PL,u,DU,VU,0);
    COE_L1(:,ii) = State2Coe(S_sci_L1,GM_central);
end

COE_VL(1,:) = COE_VL(1,:)/DU;
COE_PL(1,:) = COE_PL(1,:)/DU;
COE_L1(1,:) = COE_L1(1,:)/DU;
%% Plot sma ecc and inc
figure;
subplot(1,3,1)
hold on

% Plot SMA\

plot(Jacobi_Save_VL,ones(1,length(Jacobi_Save_VL)),'ko','MarkerSize',11)

plot(Jacobi_Save_PL,COE_PL(1,:),'kx','MarkerSize',11)


xlabel('Jacobi''s Constant')
ylabel('Semi-major axis [DU]');
set(gca,'fontsize',14)
legend('L4 Vertical','L4 Planar')
%% Plot Eccen

subplot(1,3,2)
hold on
plot(Jacobi_Save_VL,COE_VL(2,:),'ko','MarkerSize',11)

plot(Jacobi_Save_PL,COE_PL(2,:),'kx','MarkerSize',11)


xlabel('Jacobi''s Constant')
ylabel('Eccentricity');
set(gca,'fontsize',14)
%%
 subplot(1,3,3)
% Plot SMA
hold on
plot(Jacobi_Save_VL,COE_VL(3,:),'ko','MarkerSize',11)

plot(Jacobi_Save_PL,COE_PL(3,:),'kx','MarkerSize',11)


xlabel('Jacobi''s Constant')
ylabel('Inclination [deg]');
set(gca,'fontsize',14)